/**
 * 2024/03/15
 * ssyyw
 * mavproxy_ros1的飞行控制节点，根据当前飞行计划或指令，输出飞行控制命令到mavros
 *
 */

#ifndef FLIGHT_CONTROLLER
#define FLIGHT_CONTROLLER

#include <string>
#include <thread>
#include <atomic>
#include <chrono>
#include <functional>
#include <unordered_map>
#include <ros/ros.h>
#include <cstdlib>
#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/PositionTarget.h>  
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandHome.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/NavSatFix.h>
#include <mavros_msgs/CommandLong.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "drone_defs.h"

typedef std::function<int()> CommandExecuter;

class FlightController
{
public:
    FlightController();
    ~FlightController(){};

    ros::NodeHandle nh;

    void InitilizeNode();

    // TODO: 通过环境变量来区分是PX4还是APM，然后决定具体操作
    // char* fmu_version;

private:
    int HandleCmd();
    bool RunReceiveCmd(mavproxy_ros1::FlightControll::Request &req, mavproxy_ros1::FlightControll::Response &res);
    int LetDroneSetHome();
    int LetDroneReturn();
    int LetDroneRotate();
    int LetDroneHover();
    int LetDroneFlyToTargetGpsPosition();
    int LetDroneFlyToTargetPosition();
    int LetDroneFlyToTargetPositionWithOutOBS();
    int LetDroneLand();
    int LetDroneTakeoff();
    int LetDroneStop();
    int ArmDrone(bool arm);
    int SwitcheModeToTarget(const std::string &mode_name);
    int SwitcheModeToTarget();
    int ArmAndCheckoutMode(const std::string &target_mode);
    int GetTargetGpsPosition();
    int GetTargetLocalPosition();
    int GetTargetPlannerGoal();

    int SendMessage(const ros::ServiceClient &client);

    void StateCallback(const mavros_msgs::State::ConstPtr &msg);
    void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void VelocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);
    void CmdCallback(const mavproxy_ros1::FlightCmd::ConstPtr &msg);
    void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &msg);

    std::unordered_map<std::string, CommandExecuter> cmdExecuters;

    ros::Subscriber drone_state_sub;
    ros::Subscriber drone_current_pos_sub;
    ros::Subscriber drone_current_gpspos_sub;
    ros::Subscriber drone_current_velocity_sub;
    ros::Subscriber drone_flight_cmd_sub;

    ros::Publisher drone_target_pose_pub;
    ros::Publisher drone_target_gpose_pub;
    ros::Publisher drone_target_velocity_pub;
    ros::Publisher drone_target_attitude_pub;
    ros::Publisher drone_current_flight_state_pub;
    ros::Publisher drone_target_goal_pub;

    ros::ServiceClient drone_arming_client;
    ros::ServiceClient drone_set_home_client;
    ros::ServiceClient drone_mode_client;
    ros::ServiceClient drone_takeoff_cmd_client;
    ros::ServiceClient drone_land_cmd_client;

    ros::ServiceServer service;

    mavros_msgs::State drone_current_state;
    geographic_msgs::GeoPoseStamped drone_current_gpose;
    geometry_msgs::PoseStamped drone_current_pose;
    geometry_msgs::TwistStamped drone_current_velocity;

    geometry_msgs::QuaternionStamped command_long_srv;
    mavros_msgs::CommandLong return_home_srv;

    geometry_msgs::PoseStamped drone_target_pose;
    geographic_msgs::GeoPoseStamped drone_target_gpose;
    geometry_msgs::TwistStamped drone_target_velocity;
    geometry_msgs::TwistStamped drone_target_attitude;
    geometry_msgs::PoseStamped drone_target_goal;

    mavproxy_ros1::FlightCmd drone_last_cmd;
    mavproxy_ros1::FlightCmd drone_current_cmd;
};

#endif